#include "ThataStar.h"
#include "../core/AStarNode.h"
#include "../PathFinder.h"
#include "../core/BinaryHeap.h"
#include "../core/heuristics.h"


ThataStar::ThataStar(void)
{
}


ThataStar::~ThataStar(void)
{
}

bool ThataStar::lineOfSight( AStarNode* n,AStarNode* neighbour,PathFinder* finder )
{
	if(!n || !neighbour || !finder)	return false;
	int x0 = n->x;
	int y0 = n->y;
	int x1 = neighbour->x;
	int y1 = neighbour->y;
	int dx = abs(x1-x0);
	int dy = abs(y1-y0);
	int err = dx - dy;
	int sx = x0 < x1 ? 1 : -1;
	int sy = y0 < y1 ? 1 : -1;
	
	while(1)
	{
		if (!finder->grid->isWalkAbleAt(x0,y0)) return false;

		if(x0 == x1 || y0 == y1) break;

		int e2 = 2*err;
		if (e2 > -dy)
		{
			err -= dy;
			x0 += sx;
		}
		if (e2 < dx)
		{
			err += dx;
			y0 += sy;
		}
	}

	return true;
}

void ThataStar::computeCost( AStarNode* n,AStarNode* neighbour,PathFinder* finder )
{
	AStarNode* parent = NULL;
	if (n->parent)
		parent = n->parent;
	else
		parent = n;
	float mpCost = euclidian(neighbour,parent);

	if (lineOfSight(parent,neighbour,finder))
	{
		if (parent->g + mpCost < neighbour->g)
		{
			neighbour->parent = parent;
			neighbour->g = parent->g + mpCost;
		}
	}
	else
	{
		float mCost = euclidian(neighbour,n);
		if (n->g + mCost < neighbour->g)
		{
			neighbour->parent = n;
			neighbour->g = n->g + mCost;
		}
	}
}
